/* Fixed-Point math Q16.16
 * version 1.07
 **/

#include "algo_qfp16.h"
// #include <ctype.h>
// #include <stdbool.h>

/*used only for internal operations*/
/*! @cond  */
struct _qFP16_intern_s {
    const qFP16_t exp_max, f_2, /* [ 2 ] */
        f_3,                    /* [ 3 ] */
        f_16,                   /* [ -16 ] */
        f_100,                  /* [ 100 ] */
        f_6_5;                  /* [ 6.5 ] */
    const float    one_fp16_f;  /* [ 1/65536 ] */
    const double   one_fp16_d;  /* [ 1/65536 ] */
    const uint32_t overflow_mask, fraction_mask, integer_mask;
};

static const struct _qFP16_intern_s intern = {
    /*exp_max*/ 681391,
    /*f_2*/ 131072,
    /*f_3*/ 196608,
    /*f_16*/ 1048576,
    /*f_100*/ 6553600,
    /*f_6_5*/ 425984,
    /*one_fp16_f*/ 0.0000152587890625f,
    /*one_fp16_d*/ 0.0000152587890625,
    /*overflow_mask*/ 0x80000000u,
    /*fraction_mask*/ 0x0000FFFFu,
    /*integer_mask*/ 0xFFFF0000u
};

const struct _qFP16_const_s qFP16 = {
    /*f_e*/ 178145,
    /*f_log2e*/ 94548,
    /*f_log10e*/ 28462,
    /*f_ln2*/ 45426,
    /*f_ln10*/ 150902,
    /*f_pi*/ 205887,
    /*f_pi_2*/ 102944,
    /*f_2pi*/ 411775,
    /*f_pi_4*/ 51471,
    /*f_1_pi*/ 20861,
    /*f_2_pi*/ 41722,
    /*f_2_sqrtpi*/ 73949,
    /*f_sqrt2*/ 92682,
    /*f_sqrt1_2*/ 46341,
    /*epsilon*/ 1,
    /*max*/ 2147483647,
    /*overflow*/ -2147483648,
    /*one*/ 65536,
    /*one_half*/ 32768,
    /*f_180_pi*/ 3754936,
    /*f_pi_180*/ 1144,
    /*f_180*/ 11796480,
    /*f_360*/ 23592960,
};

static qFP16_Settings_t  fp_default = { -2147483647, 2147483647, 1u, 1u };
static qFP16_Settings_t *fp         = &fp_default;

/*! @endcond  */

static uint32_t qFP16_OverflowCheck(uint32_t res, const uint32_t x, const uint32_t y);
static qFP16_t  qFP16_rs(const qFP16_t x);
static qFP16_t  qFP16_log2i(qFP16_t x);
static char    *qFP16_itoa(char *buf, uint32_t scale, uint32_t value, ct_u8 skip);
static qFP16_t  qFP16_Saturate(const qFP16_t nsInput, const qFP16_t x, const qFP16_t y);

/*cstat -MISRAC2012-Rule-10.8 -CERT-FLP34-C -MISRAC2012-Rule-1.3_n -MISRAC2012-Rule-10.1_R6 -ATH-shift-neg
 * -CERT-INT34-C_c*/

/*============================================================================*/
int qFP16_SettingsSet(qFP16_Settings_t *const instance, qFP16_t min, qFP16_t max, ct_u8 rounding,
                      ct_u8 saturate) {
    int retValue = 0;

    if ((NULL != instance) && (max > min) && (rounding <= 1u) && (saturate <= 1u)) {
        instance->min      = min;
        instance->max      = max;
        instance->rounding = rounding;
        instance->saturate = saturate;
        retValue           = 1;
    } else {
        // (void)fp_default;
    }

    return retValue;
}
/*============================================================================*/
void qFP16_SettingsSelect(qFP16_Settings_t *const instance) {
    if (NULL == instance) {
        fp = &fp_default;
    } else {
        fp = instance;
    }
}
/*============================================================================*/
int qFP16_FPToInt(const qFP16_t x) {
    int retValue;

    if (1u == fp->rounding) {
        if (x >= 0) {
            /*cppcheck-suppress misra-c2012-10.1 */
            retValue = (x + (qFP16.one >> 1)) / qFP16.one;
        } else {
            /*cppcheck-suppress misra-c2012-10.1 */
            retValue = (x - (qFP16.one >> 1)) / qFP16.one;
        }
    } else {
        retValue = (int)((uint32_t)x >> 16);
    }

    return retValue;
}
/*============================================================================*/
qFP16_t qFP16_IntToFP(const int x) {
    return (qFP16_t)((uint32_t)x << 16);
}
/*============================================================================*/
qFP16_t qFP16_FloatToFP(const float x) {
    float retValue;
    /*cstat -CERT-FLP36-C*/
    retValue = x * (float)qFP16.one;
    /*cstat +CERT-FLP36-C*/
    if (1u == fp->rounding) { retValue += (retValue >= 0.0f) ? 0.5f : -0.5f; }

    return (qFP16_t)retValue;
}
/*============================================================================*/
float qFP16_FPToFloat(const qFP16_t x) {
    /*cstat -CERT-FLP36-C*/
    return (float)x * intern.one_fp16_f;
    /*cstat +CERT-FLP36-C*/
}
/*============================================================================*/
qFP16_t qFP16_DoubleToFP(const double x) {
    double retValue;
    /*cstat -CERT-FLP36-C*/
    retValue = x * (double)qFP16.one;
    /*cstat +CERT-FLP36-C*/
    if (1u == fp->rounding) { retValue += (retValue >= 0.0) ? 0.5 : -0.5; }

    return (qFP16_t)retValue;
}
/*============================================================================*/
double qFP16_FPToDouble(const qFP16_t x) {
    /*cstat -CERT-FLP36-C*/
    return (double)x * intern.one_fp16_d;
    /*cstat +CERT-FLP36-C*/
}
/*============================================================================*/
qFP16_t qFP16_Abs(const qFP16_t x) {
    qFP16_t retValue;

    if (x == fp->min) {
        retValue = qFP16.overflow;
    } else {
        retValue = (x >= 0) ? x : -x;
    }

    return retValue;
}
/*============================================================================*/
qFP16_t qFP16_Floor(const qFP16_t x) {
    /*cppcheck-suppress misra-c2012-10.8 */
    return (qFP16_t)((uint32_t)x & intern.integer_mask);
}
/*============================================================================*/
qFP16_t qFP16_Ceil(const qFP16_t x) {
    /*cstat -MISRAC2012-Rule-10.1_R2*/
    return (x & (qFP16_t)intern.integer_mask) + ((x & (qFP16_t)intern.fraction_mask) ? qFP16.one : 0);
    /*cstat +MISRAC2012-Rule-10.1_R2*/
}
/*============================================================================*/
qFP16_t qFP16_Round(const qFP16_t x) {
    return qFP16_Floor(x + qFP16.one_half);
}
/*============================================================================*/
qFP16_t qFP16_min(qFP16_t x, qFP16_t y) {
    return (x < y ? x : y);
}
/*============================================================================*/
qFP16_t qFP16_max(qFP16_t x, qFP16_t y) {
    return (x > y ? x : y);
}
/*============================================================================*/
qFP16_t qFP16_clamp(qFP16_t x, qFP16_t lo, qFP16_t hi) {
    return qFP16_min(qFP16_max(x, lo), hi);
}
/*============================================================================*/

qFP16_t qFP16_Add(const qFP16_t X, const qFP16_t Y) {
    uint32_t x = (uint32_t)X, y = (uint32_t)Y;
    uint32_t retValue;

    retValue = x + y;
    retValue = qFP16_OverflowCheck(retValue, x, y);

    return qFP16_Saturate((qFP16_t)retValue, X, X);
}
/*============================================================================*/
// the original subtraction function
qFP16_t qFP16_Sub_Original(const qFP16_t X, const qFP16_t Y) {
    uint32_t x = (uint32_t)X, y = (uint32_t)Y;
    uint32_t retValue;

    retValue = x - y;
    retValue = qFP16_OverflowCheck(retValue, x, y);

    return qFP16_Saturate((qFP16_t)retValue, X, X);
}

// Now the subtraction function is signed Davidxu 20231208
qFP16_t qFP16_Sub(const qFP16_t X, const qFP16_t Y) {
    qFP16_t retValue;

    if (Y == fp->min) {
        retValue = fp->max;
    } else {
        retValue = -Y;
    }
    retValue = qFP16_Add(X, retValue);

    return retValue;
}


// Now the subtraction function is signed Davidxu 20231031
// qFP16_t qFP16_SubSign(const qFP16_t X, const qFP16_t Y) {
//     qFP16_t retValue;

//     retValue = X - Y;


//     if (X > 0 && Y < 0 && retValue < 0) {
//         retValue = qFP16.max;
//     } else if (X < 0 && Y > 0 && retValue > 0) {
//         retValue = qFP16.overflow;
//     }
//     // 可能没有处理所有的溢出情况，特别是当X和Y都是负数时。
//     return qFP16_Saturate(retValue, X, Y);
// }

/*============================================================================*/
/* 32-bit implementation of fix16_mul. Potentially fast on 16-bit processors,
 * and this is a relatively good compromise for compilers that do not support
 * uint64_t. Uses 16*16->32bit multiplications.
 */
#if !defined(FIXMATH_OPTIMIZE_8BIT)
qFP16_t qFP16_Mul(const qFP16_t x, const qFP16_t y) {
    qFP16_t  retValue = qFP16.overflow;
    int32_t  a, c, ac, adcb, mulH;
    uint32_t b, d, bd, tmp, mulL;
    /*cstat -MISRAC2012-Rule-10.3*/
    /*cppcheck-suppress misra-c2012-10.1 */
    a = (x >> 16);
    /*cppcheck-suppress misra-c2012-10.1 */
    c = (y >> 16);
    b = (x & 0xFFFF);
    d = (y & 0xFFFF);
    /*cstat +MISRAC2012-Rule-10.3*/
    ac = a * c;
    /*cppcheck-suppress misra-c2012-10.8 */
    adcb = (int32_t)(((uint32_t)a * d) + ((uint32_t)c * b));
    bd   = b * d;
    /*cppcheck-suppress misra-c2012-10.1 */
    mulH = ac + (adcb >> 16);
    tmp  = (uint32_t)adcb << 16;
    mulL = bd + tmp;
    if (mulL < bd) { ++mulH; }
    /*cppcheck-suppress misra-c2012-10.6 */
    a = (mulH < 0) ? -1 : 0;
    /*cppcheck-suppress misra-c2012-10.1 */
    if (a == (mulH >> 15)) {
        if (1u == fp->rounding) {
            uint32_t tmp2;

            tmp2 = mulL;
            mulL -= (uint32_t)qFP16.one_half;
            mulL -= (uint32_t)mulH >> 31;
            if (mulL > tmp2) { --mulH; }
            /*cppcheck-suppress misra-c2012-10.1 */
            retValue = (qFP16_t)(mulH << 16) | (qFP16_t)(mulL >> 16);
            retValue += 1;
        } else {
            /*cppcheck-suppress misra-c2012-10.1 */
            retValue = (qFP16_t)(mulH << 16) | (qFP16_t)(mulL >> 16);
        }
    }

    return qFP16_Saturate(retValue, x, y);
}
#endif /* !defined(FIXMATH_OPTIMIZE_8BIT) */

/* 8-bit implementation of fix16_mul. Fastest on e.g. 51 AVR.
 * Uses 8*8->16bit multiplications, and also skips any bytes that are zero.
 */
#if defined(FIXMATH_OPTIMIZE_8BIT)
qFP16_t qFP16_Mul(const qFP16_t x, const qFP16_t y) {
    uint32_t _a  = qFP16_Abs(x);
    uint32_t _b  = qFP16_Abs(y);
    uint32_t low = 0;
    uint32_t mid = 0;
    qFP16_t  retValue;

    // uint8_t va[4] = { _a, (_a >> 8), (_a >> 16), (_a >> 24) };
    // uint8_t vb[4] = { _b, (_b >> 8), (_b >> 16), (_b >> 24) };

    uint8_t va[4], vb[4];

    va[0] = _a;
    va[1] = (_a >> 8);
    va[2] = (_a >> 16);
    va[3] = (_a >> 24);

    vb[0] = _b;
    vb[1] = (_b >> 8);
    vb[2] = (_b >> 16);
    vb[3] = (_b >> 24);

    // retValue column i depends on va[0..i] and vb[i..0]

    if (1u == fp->saturate) {
        // i = 6
        if (va[3] && vb[3]) return qFP16.overflow;
    }

    // i = 5
    if (va[2] && vb[3]) mid += (uint16_t)va[2] * vb[3];
    if (va[3] && vb[2]) mid += (uint16_t)va[3] * vb[2];
    mid <<= 8;

    // i = 4
    if (va[1] && vb[3]) mid += (uint16_t)va[1] * vb[3];
    if (va[2] && vb[2]) mid += (uint16_t)va[2] * vb[2];
    if (va[3] && vb[1]) mid += (uint16_t)va[3] * vb[1];

    if (1u == fp->saturate) {
        if (mid & 0xFF000000) return qFP16.overflow;
    }
    mid <<= 8;

    // i = 3
    if (va[0] && vb[3]) mid += (uint16_t)va[0] * vb[3];
    if (va[1] && vb[2]) mid += (uint16_t)va[1] * vb[2];
    if (va[2] && vb[1]) mid += (uint16_t)va[2] * vb[1];
    if (va[3] && vb[0]) mid += (uint16_t)va[3] * vb[0];

    if (1u == fp->saturate) {
        if (mid & 0xFF000000) return qFP16.overflow;
    }
    mid <<= 8;

    // i = 2
    if (va[0] && vb[2]) mid += (uint16_t)va[0] * vb[2];
    if (va[1] && vb[1]) mid += (uint16_t)va[1] * vb[1];
    if (va[2] && vb[0]) mid += (uint16_t)va[2] * vb[0];

    // i = 1
    if (va[0] && vb[1]) low += (uint16_t)va[0] * vb[1];
    if (va[1] && vb[0]) low += (uint16_t)va[1] * vb[0];
    low <<= 8;

    // i = 0
    if (va[0] && vb[0]) low += (uint16_t)va[0] * vb[0];

    // #ifndef FIXMATH_NO_ROUNDING
    // low += 0x8000;
    // #endif
    // mid += (low >> 16);

    if (1u == fp->rounding) {
        low += 0x8000;
    } else {
        mid += (low >> 16);
    }

    if (1u == fp->saturate) {
        if (mid & 0x80000000)
            return qFP16.overflow;
    }

    retValue = mid;

    /* Figure out the sign of retValue */
    if ((x >= 0) != (y >= 0)) {
        retValue = -retValue;
    }

    // return retValue;
    return qFP16_Saturate(retValue, x, y);
}

#endif /* defined(FIXMATH_OPTIMIZE_8BIT) */
/*============================================================================*/
/* 32-bit implementation of fix16_div. Fastest version for e.g. ARM Cortex M3.
 * Performs 32-bit divisions repeatedly to reduce the remainder. For this to
 * be efficient, the processor has to have 32-bit hardware division.
 */
#if !defined(FIXMATH_NO_HARD_DIVISION)

#ifdef __GNUC__
// Count leading zeros, using processor-specific instruction if available.
#define clz(x) (__builtin_clzl(x) - (8 * sizeof(long) - 32))
#else
static uint8_t clz(uint32_t x) {
    uint8_t result = 0;
    if (x == 0) return 32;
    while (!(x & 0xF0000000)) {
        result += 4;
        x <<= 4;
    }
    while (!(x & 0x80000000)) {
        result += 1;
        x <<= 1;
    }
    return result;
}
#endif

qFP16_t qFP16_Div(const qFP16_t x, const qFP16_t y) {
    uint32_t remainder, divider, shifted_div, tmp;
    uint64_t quotient;
    int      bit_pos, shift;
    uint32_t div;
    qFP16_t  retValue = fp->min;


    // 所有变量在函数开始处定义
    remainder = (uint32_t)((x >= 0) ? x : -x);
    divider   = (uint32_t)((y >= 0) ? y : -y);
    quotient  = 0;
    bit_pos   = 17;


    // 检查除数是否为0
    if (divider != 0) {
        // Kick-start the division a bit.
        if (divider & 0xFFF00000) {
            shifted_div = ((divider >> 17) + 1);
            quotient    = remainder / shifted_div;
            tmp         = ((uint64_t)quotient * (uint64_t)divider) >> 17;
            remainder -= (uint32_t)(tmp);
        }

        // If the divider is divisible by 2^n, take advantage of it.
        while (!(divider & 0xF) && bit_pos >= 4) {
            divider >>= 4;
            bit_pos -= 4;
        }

        while (remainder && bit_pos >= 0) {
            // Shift remainder as much as we can without overflowing
            shift = clz(remainder);
            if (shift > bit_pos) shift = bit_pos;
            remainder <<= shift;
            bit_pos -= shift;

            div       = remainder / divider;
            remainder = remainder % divider;
            quotient += (uint64_t)div << bit_pos;

            if (1u == fp->saturate) {
                if (div & ~(0xFFFFFFFF >> bit_pos)) {
                    return qFP16.overflow;
                }
            }

            remainder <<= 1;
            bit_pos--;
        }

        if (1u == fp->rounding) {
            // Quotient is always positive so rounding is easy
            quotient++;
        }

        retValue = quotient >> 1;

        // Figure out the sign of the retValue
        if ((x ^ y) & 0x80000000) {
            if (1u == fp->saturate) {
                if (retValue == (uint32_t)fp->min) {

                    return qFP16.overflow;
                    ;
                }
            }

            retValue = -retValue;
        }
    }

    return qFP16_Saturate(retValue, x, y);
}

#endif /* !defined(FIXMATH_NO_HARD_DIVISION) */


/* Alternative 32-bit implementation of fix16_div. Fastest on e.g. Atmel AVR.
 * This does the division manually, and is therefore good for processors that
 * do not have hardware division.
 */
#if defined(FIXMATH_NO_HARD_DIVISION)
qFP16_t qFP16_Div(const qFP16_t x, const qFP16_t y) {
    qFP16_t retValue = fp->min;

    if (0 != y) {
        uint32_t xRem, xDiv, Bit = 0x10000u;

        xRem = (uint32_t)((x >= 0) ? x : -x);
        xDiv = (uint32_t)((y >= 0) ? y : -y);

        while (xDiv < xRem) {
            xDiv <<= 1;
            Bit <<= 1;
        }
        retValue = qFP16.overflow;
        /*cstat -MISRAC2012-Rule-14.3_a*/
        if (0u != Bit) { /*MISRAC2012-Rule-14.3_a false positive*/
                         /*cstat +MISRAC2012-Rule-14.3_a*/
            uint32_t quotient = 0u;

            if (0u != (xDiv & 0x80000000u)) {
                if (xRem >= xDiv) {
                    quotient |= Bit;
                    xRem -= xDiv;
                }
                xDiv >>= 1;
                Bit >>= 1;
            }

            while ((0u != Bit) && (0u != xRem)) {
                if (xRem >= xDiv) {
                    quotient |= Bit;
                    xRem -= xDiv;
                }
                xRem <<= 1;
                Bit >>= 1;
            }
            if (1u == fp->rounding) {
                if (xRem >= xDiv) { ++quotient; }
            }

            retValue = (qFP16_t)quotient;
            /*cppcheck-suppress misra-c2012-10.8 */
            if (0u != ((uint32_t)(x ^ y) & intern.overflow_mask)) {
                if (quotient == (uint32_t)fp->min) {
                    retValue = qFP16.overflow;
                } else {
                    retValue = -retValue;
                }
            }
        }
    }

    return qFP16_Saturate(retValue, x, y);
}
#endif /*defined(FIXMATH_NO_HARD_DIVISION) */

/*============================================================================*/
static qFP16_t mod_double(qFP16_t x, qFP16_t y) {
    // Variable declarations as per C89 standard
    qFP16_t y_positive, temp_y, multiplier;
    qFP16_t fix16_overflow = qFP16.overflow;

    // Prevent division by zero
    if (y == 0) return 0;

    // Ensure y is positive
    y_positive = (y < 0) ? -y : y;

    // Use doubling method to speed up the subtraction process when x is much larger than y
    while (x >= y_positive) {
        temp_y     = y_positive;
        multiplier = 1;

        // Double temp_y until it is close to the value of x
        while (x >= (temp_y << 1)) {
            if (temp_y > (fix16_overflow >> 1)) break;    // Prevent overflow

            temp_y <<= 1;
            multiplier <<= 1;
        }

        // Subtract the doubled value of y
        x -= temp_y;

        // If we have reduced x to less than the original y, we can stop
        if (x < y_positive) break;
    }

    // Adjust x if it is negative by adding y
    while (x < 0) {
        x += y_positive;
    }

    return x;
}

qFP16_t qFP16_Mod(const qFP16_t x, const qFP16_t y) {
    qFP16_t retValue = 0;

#ifdef FIXMATH_NO_HARD_DIVISION
    /* The reason we do this, rather than use a modulo operator
     * is that if you don't have a hardware divider, this will result
     * in faster operations when the angles are close to the bounds.
     */
    // while (x >= y)
    //     x -= y;
    // while (x <= -y)
    //     x += y;
    // retValue = x;
    retValue = mod_double(x, y);
#else
    /* Note that in C90, the sign of result of the modulo operation is
     * undefined. in C99, it's the same as the dividend (aka numerator).
     */
    if (0 != y) { retValue = x % y; }
#endif /* FIXMATH_NO_HARD_DIVISION */

    return retValue;
}
/*============================================================================*/
/* The square root algorithm is quite directly from
 * http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Binary_numeral_system_.28base_2.29
 * An important difference is that it is split to two parts
 * in order to use only 32-bit operations.
 *
 * Note that for negative numbers we return -sqrt(-inValue).
 * Not sure if someone relies on this behaviour, but not going
 * to break it for now. It doesn't slow the code much overall.
 */
qFP16_t qFP16_Sqrt(qFP16_t x) {
    qFP16_t retValue = qFP16.overflow;

    if (x > 0) {
        uint32_t Bit;
        ct_u8    n;

        retValue = 0;
        // Many numbers will be less than 15, so
        // this gives a good balance between time spent
        // in if vs. time spent in the while loop
        // when searching for the starting value.
        Bit = (0 != (x & (qFP16_t)4293918720)) ? (1ul << 30ul) : (1ul << 18ul);
        while (Bit > (uint32_t)x) {
            Bit >>= 2u;
        }
        // The main part is executed twice, in order to avoid
        // using 64 bit values in computations.
        for (n = 0u; n < 2u; ++n) {
            // First we get the top 24 bits of the answer.
            while (0u != Bit) {

                if (x >= (qFP16_t)((uint32_t)retValue + Bit)) {

                    x -= (qFP16_t)((uint32_t)retValue + Bit);

                    retValue = (qFP16_t)(((uint32_t)retValue >> 1u) + Bit);
                } else {

                    retValue = (retValue >> 1);
                }
                Bit >>= 2u;
            }

            if (0u == n) {
                // Then process it again to get the lowest 8 bits.
                if (x > 65535) {
                    // The remainder 'num' is too large to be shifted left
                    // by 16, so we have to add 1 to result manually and
                    // adjust 'num' accordingly.
                    // num = a - (result + 0.5)^2
                    //	 = num + result^2 - (result + 0.5)^2
                    //	 = num - result - 0.5
                    x -= retValue;
                    x        = (x << 16) - qFP16.one_half;
                    retValue = (retValue << 16) + qFP16.one_half;
                } else {
                    x <<= 16;
                    retValue <<= 16;
                }

                Bit = 1u << 14u;
            }
        }
    }
    if ((1u == fp->rounding) && (x > retValue)) { ++retValue; }

    return (qFP16_t)retValue;
}
/*============================================================================*/
qFP16_t qFP16_Exp(qFP16_t x) {
    qFP16_t retValue, term;
    ct_bool isNegative;
    int     i;

    if (0 == x) {
        retValue = qFP16.one;
    } else if (x == qFP16.one) {
        retValue = qFP16.f_e;
    } else if (x >= intern.exp_max) {
        retValue = fp->max;
    } else if (x <= -intern.exp_max) {
        retValue = 0;
    } else {
        isNegative = (x < 0);
        if (isNegative) { x = -x; }

        retValue = x + qFP16.one;
        term     = x;

        for (i = 2; i < 30; ++i) {
            term = qFP16_Mul(term, qFP16_Div(x, qFP16_IntToFP(i)));
            retValue += term;

            if ((term < 500) && ((i > 15) || (term < 20))) { break; }
        }

        if (isNegative) { retValue = qFP16_Div(qFP16.one, retValue); }
    }

    return retValue;
}
/*============================================================================*/
qFP16_t qFP16_Log(qFP16_t x) {
    qFP16_t              retValue = qFP16.overflow;
    static const qFP16_t e4       = 3578144; /*e^4*/

    if (x > 0) {
        qFP16_t guess   = intern.f_2, delta;
        int     scaling = 0, count = 0;

        while (x > intern.f_100) {
            x = qFP16_Div(x, e4);
            scaling += 4;
        }

        while (x < qFP16.one) {
            x = qFP16_Mul(x, e4);
            scaling -= 4;
        }

        do {
            qFP16_t e = qFP16_Exp(guess);

            delta = qFP16_Div(x - e, e);

            if (delta > intern.f_3) { delta = intern.f_3; }
            guess += delta;
        } while ((count++ < 10) && ((delta > 1) || (delta < -1)));

        retValue = guess + qFP16_IntToFP(scaling);
    }

    return retValue;
}
/*============================================================================*/
qFP16_t qFP16_Log2(const qFP16_t x) {
    qFP16_t retValue = qFP16.overflow;

    if (x > 0) {
        if (x < qFP16.one) {
            if (1 == x) {
                retValue = -intern.f_16;
            } else {
                qFP16_t inv;
                inv      = qFP16_Div(qFP16.one, x);
                retValue = -qFP16_log2i(inv);
            }
        } else {
            retValue = qFP16_log2i(x);
        }
    }
    if (1u == fp->saturate) {
        if (qFP16.overflow == retValue) { retValue = fp->min; }
    }

    return retValue;
}
/*============================================================================*/
qFP16_t qFP16_RadToDeg(const qFP16_t x) {
    return qFP16_Mul(qFP16_WrapToPi(x), qFP16.f_180_pi);
}
/*============================================================================*/
qFP16_t qFP16_DegToRad(qFP16_t x) {
    return qFP16_Mul(qFP16_WrapTo180(x), qFP16.f_pi_180);
}
/*============================================================================*/
qFP16_t qFP16_WrapToPi(qFP16_t x) {
    if ((x < -qFP16.f_pi) || (x > qFP16.f_pi)) {
        while (x > qFP16.f_pi) {
            x -= qFP16.f_2pi;
        }
        while (x <= -qFP16.f_pi) {
            x += qFP16.f_2pi;
        }
    }

    return x;
}
/*============================================================================*/
qFP16_t qFP16_WrapTo180(qFP16_t x) {
    if ((x < -qFP16.f_180) || (x > qFP16.f_180)) {
        while (x > qFP16.f_180) {
            x -= qFP16.f_360;
        }
        while (x <= -qFP16.f_pi) {
            x += qFP16.f_360;
        }
    }

    return x;
}
/*============================================================================*/
qFP16_t qFP16_Sin(qFP16_t x) {
    qFP16_t retValue, x2;

    x  = qFP16_WrapToPi(x);
    x2 = qFP16_Mul(x, x);
#ifndef FIXMATH_FAST_SIN
    // Most accurate version, accurate to ~2.1%
    retValue = x;
    x        = qFP16_Mul(x, x2);
    retValue -= (x / 6); /*x^3/3!*/
    x = qFP16_Mul(x, x2);
    retValue += (x / 120); /*x^5/5!*/
    x = qFP16_Mul(x, x2);
    retValue -= (x / 5040); /*x^7/7!*/
    x = qFP16_Mul(x, x2);
    retValue += (x / 362880); /*x^9/9!*/
    x = qFP16_Mul(x, x2);
    retValue -= (x / 39916800); /*x^11/11!*/
#else
    // Fast implementation, runs at 159% the speed of above 'accurate' version
    // with an slightly lower accuracy of ~2.3%
    retValue = qFP16_Mul(-13, x2) + 546;
    retValue = qFP16_Mul(retValue, x2) - 10923;
    retValue = qFP16_Mul(retValue, x2) + 65536;
    retValue = qFP16_Mul(retValue, x);
#endif
    return retValue;
}
/*============================================================================*/
qFP16_t qFP16_Cos(qFP16_t x) {
    return qFP16_Sin(x + qFP16.f_pi_2);
}
/*============================================================================*/
qFP16_t qFP16_Tan(qFP16_t x) {
    qFP16_t a, b;

    a = qFP16_Sin(x);
    b = qFP16_Cos(x);

    return qFP16_Div(a, b);
}
/*============================================================================*/
qFP16_t qFP16_Atan2(const qFP16_t y, const qFP16_t x) {
    qFP16_t              absY, mask, angle, r, r_3;
    const qFP16_t        QFP16_0_981689 = 0x0000FB50;
    const qFP16_t        QFP16_0_196289 = 0x00003240;
    static const qFP16_t f_3pi_div_4    = 154415; /*3*pi/4*/
    /*cppcheck-suppress misra-c2012-10.1 */
    mask = (y >> (sizeof(qFP16_t) * 7u));
    absY = (y + mask) ^ mask;
    if (x >= 0) {
        r     = qFP16_Div((x - absY), (x + absY));
        angle = qFP16.f_pi_4;
    } else {
        r     = qFP16_Div((x + absY), (absY - x));
        angle = f_3pi_div_4;
    }
    r_3 = qFP16_Mul(qFP16_Mul(r, r), r);
    /* 3rd order polynomial approximation*/
    angle += qFP16_Mul(QFP16_0_196289, r_3) - qFP16_Mul(QFP16_0_981689, r);
    /*cstat -ATH-neg-check-nonneg*/
    if (y < 0) { /*ATH-neg-check-nonneg false positive*/
        angle = -angle;
    }
    /*cstat +ATH-neg-check-nonneg*/
    return angle;
}
/*============================================================================*/
qFP16_t qFP16_Atan(qFP16_t x) {
    return qFP16_Atan2(x, qFP16.one);
}
/*============================================================================*/
qFP16_t qFP16_Asin(qFP16_t x) {
    qFP16_t retValue = 0;

    if ((x <= qFP16.one) && (x >= -qFP16.one)) {
        retValue = qFP16.one - qFP16_Mul(x, x);
        retValue = qFP16_Div(x, qFP16_Sqrt(retValue));
        retValue = qFP16_Atan(retValue);
    }

    return retValue;
}
/*============================================================================*/
qFP16_t qFP16_Acos(qFP16_t x) {
    return (qFP16.f_pi_2 - qFP16_Asin(x));
}
/*============================================================================*/
qFP16_t qFP16_Cosh(qFP16_t x) {
    qFP16_t retValue = qFP16.overflow;
    qFP16_t epx, enx;

    if (0 == x) {
        retValue = qFP16.one;
    } else if ((x >= intern.exp_max) || (x <= -intern.exp_max)) {
        retValue = fp->max;
    } else {
        epx = qFP16_Exp(x);
        enx = qFP16_Exp(-x);
        if ((qFP16.overflow != epx) && (qFP16.overflow != enx)) {
            retValue = epx + enx;
            /*cppcheck-suppress misra-c2012-10.1 */
            retValue = (retValue >> 1);
        }
    }

    return retValue;
}
/*============================================================================*/
qFP16_t qFP16_Sinh(qFP16_t x) {
    qFP16_t retValue = qFP16.overflow;
    qFP16_t epx, enx;

    if (0 == x) {
        retValue = qFP16.one;
    } else if (x >= intern.exp_max) {
        retValue = fp->max;
    } else if (x <= -intern.exp_max) {
        retValue = -fp->max;
    } else {
        epx = qFP16_Exp(x);
        enx = qFP16_Exp(-x);
        if ((qFP16.overflow != epx) && (qFP16.overflow != enx)) {
            retValue = epx - enx;
            /*cppcheck-suppress misra-c2012-10.1 */
            retValue = (retValue >> 1);
        }
    }

    return retValue;
}
/*============================================================================*/
qFP16_t qFP16_Tanh(qFP16_t x) {
    qFP16_t retValue, epx, enx;

    if (0 == x) {
        retValue = 0;
    } else if (x > intern.f_6_5) { /* tanh for any x>6.5 ~= 1*/
        retValue = qFP16.one;
    } else if (x < -intern.f_6_5) { /* tanh for any x<6.5 ~= -1*/
        retValue = -qFP16.one;
    } else {
        retValue = qFP16_Abs(x);
        epx      = qFP16_Exp(retValue);
        enx      = qFP16_Exp(-retValue);
        retValue = qFP16_Div(epx - enx, epx + enx);
        retValue = (x > 0) ? retValue : -retValue;
    }

    return retValue;
}
/*============================================================================*/
qFP16_t qFP16_Polyval(const qFP16_t *const p, const size_t n, const qFP16_t x) {
    qFP16_t fx;
    size_t  i;
    /*polynomial evaluation using Horner's method*/
    fx = p[0];
    for (i = 1u; i < n; ++i) {
        qFP16_t tmp = qFP16_Mul(fx, x);

        if (qFP16.overflow == tmp) {
            fx = qFP16.overflow;
            break;
        }
        fx = qFP16_Add(tmp, p[i]);
    }

    return fx;
}
/*============================================================================*/
qFP16_t qFP16_IPow(const qFP16_t x, const qFP16_t y) {
    qFP16_t retValue;
    qFP16_t n;
    int32_t i;

    retValue = qFP16.one;
    /*cppcheck-suppress misra-c2012-10.1 */
    n = y >> 16;
    if (0 == n) {
        retValue = qFP16.one;
    } else if (qFP16.one == n) {
        retValue = x;
    } else {
        for (i = 0; i < n; ++i) {
            retValue = qFP16_Mul(x, retValue);
            if (qFP16.overflow == retValue) { break; }
        }
    }

    return retValue;
}
/*============================================================================*/
qFP16_t qFP16_Pow(const qFP16_t x, const qFP16_t y) {
    qFP16_t retValue = qFP16.overflow;

    if ((0u == ((uint32_t)y & intern.fraction_mask)) && (y > 0)) {
        /*handle integer exponent explicitly*/
        retValue = qFP16_IPow(x, y);
    } else {
        qFP16_t tmp;
        tmp = qFP16_Mul(y, qFP16_Log(qFP16_Abs(x)));
        if (qFP16.overflow != tmp) {
            retValue = qFP16_Exp(tmp);
            if (x < 0) { retValue = -retValue; }
        }
    }

    return retValue;
}
/*============================================================================*/
char *qFP16_FPToA(const qFP16_t num, char *str, int decimals) {
    char *retValue = str;

    if (qFP16.overflow == num) {
        str[0] = 'o';
        str[1] = 'v';
        str[2] = 'e';
        str[3] = 'r';
        str[4] = 'f';
        str[5] = 'l';
        str[6] = 'o';
        str[7] = 'w';
        str[8] = '\0';
    } else {
        const uint32_t iScales[6] = { 1u, 10u, 100u, 1000u, 10000u, 100000u };
        uint32_t       uValue, fPart, scale;
        int32_t        iPart;

        uValue = (uint32_t)((num >= 0) ? num : -num);
        if (num < 0) { *str++ = '-'; }

        iPart = (int32_t)(uValue >> 16);
        fPart = uValue & intern.fraction_mask;
        if (decimals > 5) { decimals = 5; }
        if (decimals < 0) { decimals = 0; }
        scale = iScales[decimals];
        fPart = (uint32_t)qFP16_Mul((qFP16_t)fPart, (qFP16_t)scale);

        if (fPart >= scale) {
            iPart++;
            fPart -= scale;
        }
        str = qFP16_itoa(str, 10000, (uint32_t)iPart, 1u);

        if (1u != scale) {
            *str++ = '.';
            str    = qFP16_itoa(str, scale / 10u, fPart, 0u);
        }
        *str = '\0';
    }

    return retValue;
}
/*============================================================================*/
ct_bool isspace(ct_s8 c) {
    if (c == ' ' || c == '\t' || c == '\n' || c == '\r' || c == '\f' || c == '\v')
        return ct_true;
    else
        return ct_false;
}

ct_bool isdigit(ct_s8 c) {
    if (c >= '0' && c <= '9')
        return ct_true;
    else
        return ct_false;
}


qFP16_t qFP16_AToFP(const char *s) {
    ct_u8    neg;
    uint32_t iPart = 0u, fPart = 0u, scale = 1u, digit;
    int32_t  count      = 0;
    qFP16_t  retValue   = qFP16.overflow;
    ct_bool  point_seen = ct_false, overflow = ct_false, badchr = ct_false;
    char     c;

    /*cstat -MISRAC2012-Dir-4.11_h*/
    while (0 != isspace((ct_s8)*s)) {
        s++; /*discard whitespaces*/
    }

    neg = ('-' == *s) ? 1u : 0u;
    if (('+' == *s) || ('-' == *s)) { s++; /*move to the next sign*/ }

    for (c = s[0]; '\0' != c; c = s[0]) {
        if ('.' == c) {
            point_seen = ct_true;
        } else if (0 != isdigit((ct_s8)c)) {
            digit = (uint32_t)c - (uint32_t)'0';
            if (point_seen) { /* Decode the fractional part */
                scale *= 10u;
                fPart *= 10u;
                fPart += digit;
            } else { /* Decode the decimal part */
                iPart *= 10u;
                iPart += digit;
                ++count;
                overflow =
                    ((0 == count) || (count > 5) || (iPart > 32768u) || ((0u == neg) && (iPart > 32767u)));
            }
        } else {
            badchr = ct_true;
        }
        if (overflow || badchr) { break; }
        s++;
    }
    if (ct_false == overflow) {
        /*cppcheck-suppress misra-c2012-10.1 */
        retValue = (qFP16_t)iPart << 16;
        retValue += qFP16_Div((qFP16_t)fPart, (qFP16_t)scale);
        retValue = (1u == neg) ? -retValue : retValue;
    }
    /*cstat +MISRAC2012-Dir-4.11_h*/
    return retValue;
}
/*============================================================================*/
static uint32_t qFP16_OverflowCheck(uint32_t res, const uint32_t x, const uint32_t y) {
    if ((0u == ((x ^ y) & intern.overflow_mask)) && (0u != ((x ^ res) & intern.overflow_mask))) {
        res = (uint32_t)qFP16.overflow;
    }

    return res;
}
/*============================================================================*/
static qFP16_t qFP16_rs(const qFP16_t x) {
    qFP16_t retValue;

    if (1u == fp->rounding) {
        /*cppcheck-suppress misra-c2012-10.1 */
        retValue = (x >> 1u) + (x & 1);
    } else {
        /*cppcheck-suppress misra-c2012-10.1 */
        retValue = x >> 1;
    }

    return retValue;
}
/*============================================================================*/
static qFP16_t qFP16_log2i(qFP16_t x) {
    qFP16_t retValue = 0;

    while (x >= intern.f_2) {
        ++retValue;
        x = qFP16_rs(x);
    }

    if (0 == x) {
        /*cppcheck-suppress misra-c2012-10.1 */
        retValue = retValue << 16;
    } else {
        int i;
        for (i = 16; i > 0; --i) {
            x = qFP16_Mul(x, x);
            retValue <<= 1;
            if (x >= intern.f_2) {
                retValue |= 1;
                x = qFP16_rs(x);
            }
        }
        if (1u == fp->rounding) {
            x = qFP16_Mul(x, x);
            if (x >= intern.f_2) { ++retValue; }
        }
    }

    return retValue;
}
/*============================================================================*/
static char *qFP16_itoa(char *buf, uint32_t scale, uint32_t value, ct_u8 skip) {
    while (0u != scale) {
        uint32_t digit = (value / scale);
        if ((0u == skip) || (0u != digit) || (1u == scale)) {
            skip = 0u;
            /*cstat -MISRAC2012-Rule-10.2 -MISRAC2012-Rule-10.3*/
            *buf++ = (char)'0' + (char)digit;
            /*cstat +MISRAC2012-Rule-10.2 +MISRAC2012-Rule-10.3*/
            value %= scale;
        }
        scale /= 10u;
    }

    return buf;
}
/*============================================================================*/
static qFP16_t qFP16_Saturate(const qFP16_t nsInput, const qFP16_t x, const qFP16_t y) {
    qFP16_t retValue = nsInput;

    if (1u == fp->saturate) {
        if (qFP16.overflow == nsInput) { retValue = ((x >= 0) == (y >= 0)) ? fp->max : fp->min; }
    }

    return retValue;
}
/*============================================================================*/
/*cstat +MISRAC2012-Rule-10.8 +CERT-FLP34-C +MISRAC2012-Rule-1.3_n +MISRAC2012-Rule-10.1_R6 +ATH-shift-neg
 * +CERT-INT34-C_c*/


#if 0

// 测试函数1
// Comment for function test1
void test1(void) {
    qFP16_t overflowValue, roundingTest;

    int     intValue   = 123;                                       // Define an integer variable named intValue and assign it the value of 123
    qFP16_t fixedValue = qFP16_IntToFP(intValue);                   // Convert intValue to qFP16_t type and assign it to fixedValue
    int     backToInt  = qFP16_FPToInt(fixedValue);                 // Convert fixedValue to int type and assign it to backToInt
    printf("Int to Fixed: %ld -> %ld\n", intValue, fixedValue);     // Print the conversion result from intValue to fixedValue
    printf("Fixed to Int: %ld -> %ld\n", fixedValue, backToInt);    // Print the conversion result from fixedValue to backToInt

    // Test for constants
    printf("Pi in Fixed: %ld\n", qFP16.f_pi);    // Print the value of the f_pi member in the qFP16 structure
    printf("E in Fixed: %ld\n", qFP16.f_e);      // Print the value of the f_e member in the qFP16 structure

    // Out of range test
    overflowValue = qFP16_Constant(200);              // Convert 200 to qFP16_t type and assign it to overflowValue
    printf("Overflow Test: %ld\n", overflowValue);    // Print the value of overflowValue

    roundingTest = qFP16_Constant(0.6);              // Convert 0.6 to qFP16_t type and assign it to roundingTest
    printf("Rounding Test: %ld\n", roundingTest);    // Print the value of roundingTest
}


static void test2(void) {
    qFP16_Settings_t settingsA, settingsB;
    qFP16_t          value, largeValue;
    int              resultA, resultB;
    // Configure settings A with overflow protection enabled and rounding disabled
    qFP16_SettingsSet(&settingsA, -32768, 32767, 1, 0);

    // Configure settings A with overflow protection enabled and rounding disabled
    qFP16_SettingsSet(&settingsB, -32768, 32767, 0, 1);

    // Select settings A
    qFP16_SettingsSelect(&settingsA);
    value   = qFP16_Constant(0.6);
    resultA = qFP16_FPToInt(value);
    printf("Result with settings A: %ld\n", resultA);

    // Select settings B
    qFP16_SettingsSelect(&settingsB);
    largeValue = qFP16_Constant(50000);
    resultB    = qFP16_FPToInt(largeValue);
    printf("Result with settings B: %ld\n", resultB);
}


static void test3(void) {
    // Test converting floating-point numbers to fixed-point numbers and back

    float   convertedFloat, floatValue   = -0.25f;
    double  convertedDouble, doubleValue = 0.75;
    qFP16_t fixedPointValueFromFloat, fixedPointValueFromDouble, absValue;

    fixedPointValueFromFloat  = qFP16_FloatToFP(floatValue);      // Convert floating-point number to fixed-point number
    fixedPointValueFromDouble = qFP16_DoubleToFP(doubleValue);    // Convert double-precision floating-point number to fixed-point number

    // Print the fixed-point numbers
    printf("Fixed-point value from float: %ld\n", fixedPointValueFromFloat);
    printf("Fixed-point value from double: %ld\n", fixedPointValueFromDouble);

    absValue = qFP16_Abs(fixedPointValueFromFloat);    // Calculate the absolute value of the fixed-point number

    // Print the absolute value
    printf("Absolute value: %ld\n", absValue);

    convertedFloat  = qFP16_FPToFloat(fixedPointValueFromFloat);      // Convert fixed-point number back to floating-point number
    convertedDouble = qFP16_FPToDouble(fixedPointValueFromDouble);    // Convert fixed-point number back to double-precision floating-point number

    // Print the converted floating-point numbers
    printf("Converted back to float: %f\n", convertedFloat);
    printf("Converted back to double: %lf\n", convertedDouble);
}


static void test4(void) {
    // ??????????????
    float   floatValue = 2.49;
    qFP16_t fixedPointValue, floorValue, ceilValue, roundValue;
    // ??????????????????
    fixedPointValue = qFP16_FloatToFP(floatValue);
    printf("source value: %ld\n", fixedPointValue);

    // ???????
    floorValue = qFP16_Floor(fixedPointValue);
    printf("Floor value: %ld\n", floorValue);

    // ???????
    ceilValue = qFP16_Ceil(fixedPointValue);
    printf("Ceil value: %ld\n", ceilValue);

    // ????????
    roundValue = qFP16_Round(fixedPointValue);
    printf("Round value: %ld\n", roundValue);
}

static void test5(void) {
    // Test function 5
    float floatA = 3.5;     // Define float variable floatA and assign the value 3.5
    float floatB = 1.25;    // Define float variable floatB and assign the value 1.25

    qFP16_t fixedPointA, fixedPointB, resultAdd, resultSub, resultMul, resultDiv, resultMod;    // Define 16-bit unsigned integer fixed point variables

    // Convert floating-point numbers to fixed point
    fixedPointA = qFP16_FloatToFP(floatA);          // Convert floatA to fixed point and assign the result to fixedPointA
    fixedPointB = qFP16_FloatToFP(floatB);          // Convert floatB to fixed point and assign the result to fixedPointB
    printf("source valueA: %ld\n", fixedPointA);    // Print the source value A
    printf("source valueB: %ld\n", fixedPointB);    // Print the source value B

    // Addition operation
    resultAdd = qFP16_Add(fixedPointA, fixedPointB);    // Perform addition operation on fixedPointA and fixedPointB using qFP16_Add function and assign the result to resultAdd
    printf("Addition result: %ld\n", resultAdd);        // Print the addition result

    // Subtraction operation
    resultSub = qFP16_Sub(fixedPointA, fixedPointB);    // Perform subtraction operation on fixedPointA and fixedPointB using qFP16_Sub function and assign the result to resultSub
    printf("Subtraction result: %ld\n", resultSub);     // Print the subtraction result
    resultSub = qFP16_Sub(fixedPointB, fixedPointA);    // Perform subtraction operation on fixedPointB and fixedPointA using qFP16_Sub function and assign the result to resultSub
    printf("Subtraction result: %ld\n", resultSub);     // Print the subtraction result

    // Multiplication operation
    resultMul = qFP16_Mul(fixedPointA, fixedPointB);      // Perform multiplication operation on fixedPointA and fixedPointB using qFP16_Mul function and assign the result to resultMul
    printf("Multiplication result: %ld\n", resultMul);    // Print the multiplication result

    // Division operation
    resultDiv = qFP16_Div(fixedPointA, fixedPointB);    // Perform division operation on fixedPointA and fixedPointB using qFP16_Div function and assign the result to resultDiv
    printf("Division result: %ld\n", resultDiv);        // Print the division result

    // Modulo operation
    resultMod = qFP16_Mod(fixedPointA, fixedPointB);    // Perform modulo operation on fixedPointA and fixedPointB using qFP16_Mod function and assign the result to resultMod
    printf("Modulo result: %ld\n", resultMod);          // Print the modulo result
}

static void test6(void) {
    // ??????????????
    float   floatValue = 4.0;
    qFP16_t fixedPointValue, sqrtValue, expValue, logValue, log2Value;

    // ??????????????????
    fixedPointValue = qFP16_FloatToFP(floatValue);

    // ?????????
    sqrtValue = qFP16_Sqrt(fixedPointValue);
    printf("Square root: %ld\n", sqrtValue);

    // ????e??x????
    expValue = qFP16_Exp(fixedPointValue);
    printf("Exponential value: %ld\n", expValue);

    // ???????????
    logValue = qFP16_Log(fixedPointValue);
    printf("Natural logarithm: %ld\n", logValue);

    // ??????2???????
    log2Value = qFP16_Log2(fixedPointValue);
    printf("Log base 2: %ld\n", log2Value);
}



qFP16_t polynomialCoefficients[3];

void initCoefficients() {
    polynomialCoefficients[0] = qFP16_FloatToFP(1.0);
    polynomialCoefficients[1] = qFP16_FloatToFP(3.0);
    polynomialCoefficients[2] = qFP16_FloatToFP(2.0);
}

static void test7(void) {
    // ??????????????
    float   floatValue = 2.5;
    qFP16_t fixedPointValue, polyvalResult, integerPower, powerResult;
    float   convertedFloat;
    int     backToInt;
    // ???????????��?????2x^2 + 3x + 1??

    // ??????????????????
    fixedPointValue = qFP16_FloatToFP(floatValue);
    initCoefficients();


    // ????????
    polyvalResult = qFP16_Polyval(polynomialCoefficients, 2, fixedPointValue);
    printf("Polynomial evaluation result: %ld\n", polyvalResult);

    // ?????????????��2.5^2??
    integerPower = qFP16_IPow(fixedPointValue, qFP16_FloatToFP(2.0));
    printf("Integer power result: %ld\n", integerPower);

    // ?????????��2.5^1.5??
    powerResult = qFP16_Pow(fixedPointValue, qFP16_FloatToFP(1.5));
    printf("Power result: %ld\n", powerResult);

    // ???????????????????
    convertedFloat = qFP16_FPToFloat(powerResult);
    // ??????????
    printf("Converted back to float: %f\n", convertedFloat);

    // ?????????????????
    backToInt = qFP16_FPToInt(powerResult);
    printf("Fixed to Int: %ld -> %ld\n", powerResult, backToInt);    // ?????????
}

static void test8(void) {
    // ??????????????
    float       floatValue = 3.14159;
    qFP16_t     fixedPointValue, convertedValue;
    char        buffer[20];
    const char *strValue = "2.71828";
    // ??????????????????
    fixedPointValue = qFP16_FloatToFP(floatValue);
    printf("source valueA: %ld\n", fixedPointValue);

    // ??????????????????
    qFP16_FPToA(fixedPointValue, buffer, 6);    // ???4�˧�??
    printf("Converted to string: %s\n", buffer);

    // ??????????????????

    convertedValue = qFP16_AToFP(strValue);
    printf("Converted from string: %ld\n", convertedValue);
}

static void test9(void) {
    float   radianValue;
    qFP16_t fixedPointRadian, degreeValue, radianConverted, wrappedRadian, wrappedDegree;

    // ??????????????????��???180??
    radianValue = 3.14159;

    // ??????????????????
    fixedPointRadian = qFP16_FloatToFP(radianValue);

    // ????????????
    degreeValue = qFP16_RadToDeg(fixedPointRadian);
    printf("Converted to degrees: %ld\n", degreeValue);

    // ????????????
    radianConverted = qFP16_DegToRad(degreeValue);
    printf("Converted back to radians: %ld\n", radianConverted);

    // ???????[-??, ??]??��
    wrappedRadian = qFP16_WrapToPi(qFP16_FloatToFP(7 * 3.14159));    // 7??
    printf("Wrapped to [-??, ??]: %ld\n", wrappedRadian);

    // ???????[-180, 180]??��
    wrappedDegree = qFP16_WrapTo180(qFP16_FloatToFP(540));    // 540??
    printf("Wrapped to [-180, 180]: %ld\n", wrappedDegree);
}

static void test10(void) {
    float   radianValue;
    qFP16_t fixedPointRadian, sinValue, cosValue, tanValue;

    // ???????????????????/4????45??
    radianValue = 3.14159 / 4;

    // ??????????????????
    fixedPointRadian = qFP16_FloatToFP(radianValue);
    printf("source value: %ld\n", fixedPointRadian);

    // ?????????
    sinValue = qFP16_Sin(fixedPointRadian);
    printf("Sine value: %ld\n", sinValue);

    // ?????????
    cosValue = qFP16_Cos(fixedPointRadian);
    printf("Cosine value: %ld\n", cosValue);

    // ?????????
    tanValue = qFP16_Tan(fixedPointRadian);
    printf("Tangent value: %ld\n", tanValue);
}

#define INT32_MIN -2147483647
#define INT32_MAX 2147483647

// qFP16_t test_cases[][2] = {
//     { INT32_MIN, INT32_MIN },    // X Y,都为负数 1
//     { INT32_MAX, INT32_MAX },    // X, Y都为正数 2
//     { 0, 0 },                    // X, Y都为0 3
//     { INT32_MAX, INT32_MIN },    // X为正数，Y为负数 4
//     { INT32_MIN, INT32_MAX },    // X为负数，Y为正数 5
//     { INT32_MAX, 1 },            // X > Y 6
//     { 1, INT32_MAX },            // X < Y 7
//     { 12345, 12345 },            // X = Y 8
//     { -12345, 12345 },           // X < Y (负数) 9
//     { 12345, -12345 }            // X > Y (负数) 10
// };

// #define INT32_MIN -32768
// #define INT32_MAX 32767

qFP16_t test_cases[][2] = {
    { INT32_MIN, INT32_MIN },          // X Y,都为负数 1
    { INT32_MAX, INT32_MAX },          // X, Y都为正数 2
    { 0, 0 },                          // X, Y都为0 3
    { INT32_MAX, INT32_MIN },          // X为正数，Y为负数 4
    { INT32_MIN, INT32_MAX },          // X为负数，Y为正数 5
    { INT32_MAX, 2 },                  // X > Y 6
    { 2, INT32_MAX },                  // X < Y 7
    { 1024, 256 },                     // X = Y 8
    { -1024 * 65536, 256 * 65536 },    // X < Y (负数) 9
    { 1024 * 65536, -256 * 65536 },    // X > Y (负数) 10
    // 新增的测试用例
    { 1, -1 },            // X > Y (X为正数，Y为负数) 11
    { -1, 1 },            // X < Y (X为负数，Y为正数) 12
    { INT32_MAX, 1 },     // X接近最大值，Y为正数 13
    { INT32_MIN, -1 },    // X接近最小值，Y为负数 14
    { 32760, -5 },        // X为正数接近最大值，Y为负数 15
    { -32760, 5 },        // X为负数接近最小值，Y为正数 16
    { 0, INT32_MAX },     // X为0，Y为最大值 17
    { 0, INT32_MIN },     // X为0，Y为最小值 18
    { INT32_MAX, 0 },     // X为最大值，Y为0 19
    { INT32_MIN, 0 }      // X为最小值，Y为0 20
};


// 测试函数
void test_sub(void) {
    // 测试用例数组
    int i;

    // 测试每个用例
    for (i = 0; i < sizeof(test_cases) / sizeof(test_cases[0]); i++) {
        qFP16_t X               = test_cases[i][0];
        qFP16_t Y               = test_cases[i][1];
        qFP16_t result_original = qFP16_Sub_Original(X, Y);
        qFP16_t result_signed   = qFP16_Sub(X, Y);
        qFP16_t sim_sub         = qFP16_Sub(X, Y);

        qFP16_t result_Add = qFP16_Add(X, Y);
        qFP16_t result_mul = qFP16_Mul(X, Y);
        qFP16_t result_div = qFP16_Div(X, Y);
        // 打印结果
        printf("Test case %d: X = %ld, Y = %ld\n", i + 1, X, Y);
        printf("Original Subtraction: %ld\n", result_original);
        printf("Nov Subtraction: %ld\n", result_signed);
        printf("Signed Subtraction: %ld\n", sim_sub);
        printf("\n");
        printf("result Add: %ld\n", result_Add);
        printf("Multiplication: %ld\n", result_mul);
        printf("Division: %ld\n", result_div);
        printf("\n");
        printf("\n");
    }
}


void qfp16_test(void) {
    //   qFP16_Settings_t settingsA;


    //    qFP16_SettingsSet(&settingsA, -32768, 32767, 1, 0);
    //    qFP16_SettingsSelect(&settingsA);

    while (1) {

        test_sub();
    }
}

#endif